#!/usr/bin/env python3
# coding: utf8

from numpy import array, deg2rad
from math import sin, cos

from robotics.registration.util import transform_point_cloud, explain_quaternion
from robotics.registration.icp import Aligner
from robotics.registration.correspondence.oneway import find_correspondences
from robotics.registration.rotation.eigen import extract_rotation
from robotics.registration.criteria.rms import stop_condition

d = deg2rad(2)

rotation = array([
    [cos(d), -sin(d), 0],
    [sin(d), cos(d), 0],
    [0, 0, 1]
])
print('Initial:\n%s' % rotation)

source_cloud = array([
    [i, 0, 0] for i in range(100)
])

target_cloud = transform_point_cloud(source_cloud, rotation)

a = Aligner(extract_rotation, find_correspondences, stop_condition(max_iterations=100))
r, t = a.align(source_cloud, target_cloud)

print('Rotation:\n%s' % r)
print('Quaternion:\n%s,%s' % explain_quaternion(r))
print('Translation:\n%s' % t)